%% Tube NMPC scheme based on robust control invariant set with application to Two Wheeled differential robot
% Tube-based NMPC by Liang Hongpeng, 171186623@qq.com

%% Tips:
% 首先运行SDPT3-4.0下面的startup.m,否则提示找不到Solver

%% System description
% x' = f(x,u) + Bw * w

%% Initiallization
clear all % 清空所有变量
close all % 关闭所有图形窗口
clc % 清空命令行

%% Model description
% 定义机器人的参数
% L = 0.5; % 轮距m
% R = 0.1; % 轮半径m
% v_max = 0.5; % 最大线速度 0.4m/s
% w_max = 0.5; % 最大角速度 0.8rad/s
Bw = [1,0,0;0,1,0;0,0,3]; % 扰动输入矩阵

%% Problem formulation
% 定义NMPC的参数
N = 10; % 预测时域 N * dt
T = 0.6; % 仿真时长 T * dt 0.7
Q = diag([1.0, 1.0, 1.0]); % 状态权重矩阵
R = diag([1.0, 1.0]); % 输入权重矩阵
P = Q; % 终端状态权重矩阵

%% Disturbance
wmax_x = 0.006; % 状态x扰动的最大值0.03m/s 0.021
wmax_y = 0.006; % 状态y扰动的最大值0.03m/s
wmax_theta = 0.006; % 状态theta扰动的最大值0.09rad/s
wmax = [wmax_x; wmax_y; wmax_theta];
wmax_norm = norm(wmax);
W_vertex = [wmax_x , wmax_y , wmax_theta; % 扰动集合的顶点
            wmax_x , -wmax_y, wmax_theta;
            -wmax_x, wmax_y , wmax_theta; 
            -wmax_x, -wmax_y, wmax_theta;
            wmax_x , wmax_y , -wmax_theta; 
            wmax_x , -wmax_y, -wmax_theta;
            -wmax_x, wmax_y , -wmax_theta; 
            -wmax_x, -wmax_y, -wmax_theta;];
% 在这个特定的代码中，W表示一个三维的矩形，表示在xyz轴上的扰动范围
W = Polyhedron(W_vertex); % 构造扰动集合的Polyhedron对象

load('./data/test0.006.mat'); % w
%% Reference Trajectory
% 定义参考轨迹:使用五次多项式拟合给定的路径点

% trajectory1
% path_x = [0, 1, 2, 3, 4, 5, 6]; % 路径点的x坐标
% path_y = [0, 0, 1, 2, 2, 1, 0]; % 路径点的y坐标
% poly = polyfit(path_x, path_y, 5); % 拟合五次多项式
% ref_x = linspace(0, 6, 200); % 参考轨迹的x坐标
% ref_y = polyval(poly, ref_x); % 参考轨迹的y坐标
% % polyval求多项式在每一点的值/polyder求多项式的导数
% ref_theta = atan(polyval(polyder(poly), ref_x)); % 参考轨迹的偏航角

% trajectory2
% 生成圆的参数
radius = 2;  % 第一个圆的半径
center1 = [-2, 0];% 第一个圆的中心在坐标轴左边
center2 = [2, 0];% 第二个圆的中心在坐标轴右边

ref_x = [];
ref_y = [];
ref_theta = [];
ref_v = [];

% 第一段半圆
theta1 = linspace(0, pi, 50);% 生成圆的角度
adjust = pi;
[x_seq,y_seq,theta_seq] = path_generation(theta1,center1,radius,adjust);
ref_x = [ref_x,x_seq];ref_y = [ref_y,y_seq];ref_theta = [ref_theta,theta_seq];

% 第二段半圆
theta1 = linspace(pi, 2*pi, 50);% 生成圆的角度
adjust = 0;
[x_seq,y_seq,theta_seq] = path_generation(theta1,center1,radius,adjust);
ref_x = [ref_x,x_seq];ref_y = [ref_y,y_seq];ref_theta = [ref_theta,theta_seq];
x_sup = [ref_x(1:N)];y_sup = [ref_y(1:N)];theta_sup = [ref_theta(1:N)];

% 第三段半圆
theta2 = linspace(pi, 0, 50);% 生成圆的角度
adjust = 0;
[x_seq,y_seq,theta_seq] = path_generation(theta2,center2,radius,adjust);
ref_x = [ref_x,x_seq];ref_y = [ref_y,y_seq];ref_theta = [ref_theta,theta_seq];

% 第四段半圆
theta2 = linspace(0, -pi, 50);% 生成圆的角度
adjust = -pi;
[x_seq,y_seq,theta_seq] = path_generation(theta2,center2,radius,adjust);
ref_x = [ref_x,x_seq];ref_y = [ref_y,y_seq];ref_theta = [ref_theta,theta_seq];

% 第五段半圆
ref_x = [ref_x,x_sup];
ref_y = [ref_y,y_sup];
ref_theta = [ref_theta,theta_sup];

% 偏航角跳变处理
ref_theta = angleseq_bound(ref_theta);

% 计算参考速度
ref_v = calc_vel(ref_x, ref_y, T);

% 参考轨迹图
% figure()
% %'FontSize'设置所有的字的大小（刻度、坐标轴、图例等）
% %set(gca,'FontName','Times New Roman','FontSize',14);
% plot(ref_x, ref_y, 'r.', 'LineWidth', 0.8)
% 
% %设置坐标轴名称的字体，可以覆盖上述设置
% xlabel('x (m)','fontsize',16);ylabel('y (m)','fontsize',16);
% %设置图例；'Orientation'设置图例为竖着还是横着，默认为竖着，'horizontal'为横
% legend({'ref'},'Orientation','horizontal');
% %设置图例位置'NorthOutside'表示在上方外部；右上角为'NorthEast'，依此类推
% set(legend,'Location','NorthEast');
% 
% axis equal;  % 保持坐标轴比例相等
% grid on;
%% Initial settings
%x0 = [0; 0; 0];                    % 初始状态x0
x0 = [0; 0; pi/2];                  % 初始状态x0
uM = [0.5; 0.5];                    % 控制输入u的约束条件v_max,w_max
xM = [0.07; 0.07; 0.15];            % 状态误差deltax的约束 yes: 0.07; 0.07; 0.15 0.1; 0.1; 0.1   no: 0.05; 0.05; 0.5
u_0 = zeros(2,1);
last_theta_r=pi/2;

% 定义存储实际状态和控制输入的变量
ns = length(ref_x);
x_sim = x0; 
%xc_nomi_sim = x0;
xc_sim = x0;
x_sim_history = [];                 % 存储标称状态
x_sim_history(:,1) = x0;            
xc_sim_history = [];                % 存储实际状态
xc_sim_history(:,1) = x0;           
u_sim_history = [];                 % 存储标称输入
u_sim_history(:,1) = zeros(2,1);
uc_sim_history = [];                % 存储实际输入
uc_sim_history(:,1) = zeros(2,1);
tilde_x = zeros(3, ns);             % 存储实际状态-名义状态的差
ref_w = [];                % 存储参考角速度输入
times = [];
times(1) = 0;

w_nomi = zeros(3, ns);                  %存储扰动
u_opt = zeros(2,1);

%% NMPC Optimization problem using YALMIP
%for k = 1 : ns-(N-1)
for k = 1 : 30  
    % 在循环开始处记录时间
    iteration_start_time = tic;

    % Caculate the robust invariant set (RIS)=鲁棒控制不变集
    % 离散线性化差速机器人误差模型得到A和B
    [A,B] = linear_errmodel(ref_v(k), ref_theta(k), T);

    % 三个输入参数，分别是系统矩阵A、输入矩阵B和扰动输入矩阵Bw。
    % P1:鲁棒不变集的某种表示或描述。
    % K:标称系统的反馈控制率
    % lambda0:用于计算鲁棒不变集的某种参数。
    % mu:用于计算鲁棒不变集的某种参数。
    [P1, K, lambda0, mu] = NMPC_get_ris_new (A, B, Bw); 

    % 可视化鲁棒不变集（Robust Invariant Set）的形状
    % 鲁棒不变集是一种表示系统在存在扰动的情况下，状态能够保持在一定范围内的集合。
    % P是一个矩阵，表示鲁棒不变集的形状。这个矩阵通常是通过优化算法计算得到的。
    % mu*wmax_norm^2/lambda0 表示椭圆的半长轴长度，其中mu是鲁棒性的界限，wmax是扰动的最大值，lambda0是优化问题中的一个参数。 
    
    % 图5: 最小鲁棒正不变集的形状
    %draw_ellip(P1, mu * wmax_norm^2/lambda0, 'b') % 绘制椭圆
    %hold on

    % 在鲁棒控制不变集范围内的受最大程度干扰情况下控制输入u的优化计算问题
    yalmip('clear');
    x = sdpvar(3,1); % 定义优化变量
    % 约束是基于给定的特征矩阵P、mu、wmax​和lambda0。这个约束实际上是在确保x属于robust invariant set的前提下进行优化
    % 见论文Lemma 2.
    const = x' * P1 * x <= (mu * wmax_norm^2 / lambda0); % 约束条件
    %const = x' * P1 * x <= (mu * wmax.^2 / lambda0); % debug
    obj1 = K * x; % 目标函数
    % 最小化目标函数obj1，并且满足约束条件const
    ops = sdpsettings('solver','Mosek','verbose', 0); % fmincon/sdpt3/Mosek都可以
    optimize(const, obj1, ops); % 优化
    % 得到优化后的目标函数的最小值，即得到了控制输入u的最优值
    u_0 = value(obj1); % 获取优化结果

    % Get the terminal region Xf and terminal penalty for nominal MPC=终端最大鲁棒正不变集
    % 计算了终端区域Xf​和终端惩罚Pnom​的最大值，以及控制增益K0nom。这些参数是在MPC框架中使用
    
    % alphaM是alpha的上界，-1表示无上界。
    % alphaM被用作名义MPC中终端区域和终端权重的上界，通过调整这个参数，可以尝试不同的管道型MPC设计，以满足不同的性能和稳定性要求
    alphaM = 5;% 越小，tube越小 yes: 1  50 270 1000 no: 0.1
    [P_nom, K0_nom, alpha_nom] = NMPC_get_max_terminal_tube_new(Q, R, uM + u_0, xM, alphaM, A, B);
    %alpha_nom
    %P_nom

    % pause(1)
    % 绘制以Pnom​为矩阵的椭圆，用绿色点线表示。这个椭圆是终端区域Xf​的表示
    % 图5: 最大终端不变集范围
    %draw_ellip(P_nom, alpha_nom, 'g')
    %legend({'mRIS','MPI'},'Orientation','horizontal');

    % 更新参考状态
    j = k+N-1;
    ref = [ref_x(k:j); ref_y(k:j); ref_theta(k:j)];
    theta_r = ref_theta(k); w_r=angle_bound(theta_r-last_theta_r)/T; last_theta_r=theta_r;

    % 定义决策变量x和u为优化变量
    yalmip('clear');
    X = sdpvar(3, N); % 状态变量
    U = sdpvar(2, N-1); % 输入变量
    %w(:,k) = pick_random_Disturbance(W); % 从扰动集合中随机选择扰动          
        
    % 约束(硬约束:状态或输入的边界条件)
    const = [];
    for i = 1:N-1
        const = [const, X(:, i + 1) == robot_model(X(:, i), U(:, i), T)];   % 添加状态转移约束
        %const = [const, (X(:, i) - ref(:,i))' * P1 * (X(:, i) - ref(:,i)) <= (mu * wmax_norm^2 / lambda0)]; % x ∈ Xf:terminal set a 
    end
    const = [const, X(:, 1) == x0]; % 添加初始状态约束

    % normal
    %const = [const, -uM(1) <= U(1, :) <= uM(1)]; % 添加vel输入约束
    %const = [const, -uM(2) <= U(2, :) <= uM(2)]; % 添加anglevel输入约束
    % tube
    const = [const, (X(:, N) - ref(:,N))' * P_nom * (X(:, N) - ref(:,N)) <= alpha_nom]; % x ∈ Xf:terminal set
    const = [const, U(1, :) >= (-uM(1) - u_0(1)), U(1, :) <= (uM(1) + u_0(1))]; 
    const = [const, U(2, :) >= (-uM(2) - u_0(2)), U(2, :) <= (uM(2) + u_0(2))]; 
    %const = [const, abs(U(1,:)-u_opt(1)) <= 0.15];
    %const = [const, abs(U(2,:)-u_opt(2)) <= 0.15];

    % 目标函数(性能指标+惩罚项软约束)
    obj = 0;
    W1 = Q/P_nom;
    W2 = R/P_nom(1:2,1:2);
    for i=2:N-1
        e_x = X(1, i) - ref(1,i);
        e_y = X(2, i) - ref(2,i);
        e_theta = X(3, i) - ref(3,i);
        e = [e_x; e_y; e_theta];
        u = U(:, i);
        deltau = abs(U(:, i) - U(:, i-1));
        %obj = obj + e' * W1 * e + u' * W2 * u + deltau' * 0.5*W2 * deltau; % 可以减小控制量的抖动
        obj = obj + e' * W1 * e + u' * W2 * u; % Q R
    end

    % normal
    %obj = obj + (X(:,end) - ref(:,end))' * P * (X(:,end) - ref(:,end));
    % tube
    obj = obj + (X(:,end) - ref(:,end))' * Q * (X(:,end) - ref(:,end));% P_nom x ∈ Xf:terminal set

    % 求解优化问题
    ops = sdpsettings('solver', 'fmincon', 'fmincon.maxiter', 50, 'fmincon.tolx', 1e-6, 'verbose', 0); %求解器/迭代次数/容许误差/显示级别
    optimize(const, obj, ops);

    % 提取最优控制输入
    u_opt = value(U(:, 1));

    % 更新系统状态
    % normal
    % x_sim = robot_model(x_sim, u_opt, T);
    % x0 = x_sim;

    % tube
    uc_opt = u_opt + K * tilde_x(:, k);
    x_sim = robot_model(x_sim, u_opt, T); % 理想系统
    xc_sim = robot_model(xc_sim, uc_opt, T) + Bw * w(:,k); % 真实带噪声
    tilde_x(:, k+1) = xc_sim - x_sim; % 误差反馈
    x0 = x_sim;

    % 保存历史数据
    u_sim_history = [u_sim_history, u_opt];
    x_sim_history = [x_sim_history, x_sim];
    uc_sim_history = [uc_sim_history, uc_opt];
    xc_sim_history = [xc_sim_history, xc_sim];
    ref_w = [ref_w, w_r];
    times = [times, k*T];

    %k % 显示当前步数
    iteration_elapsed_time = toc(iteration_start_time);% 计算循环耗时
    fprintf('Iteration %d took %.4f seconds\n', k, iteration_elapsed_time);% 输出耗时信息

    % 图1：实际轨迹与参考轨迹对比
    figure(1)
    %'FontSize'设置所有的字的大小（刻度、坐标轴、图例等）
    %set(gca,'FontName','Times New Roman','FontSize',14);
    plot(ref_x, ref_y, 'r.', 'LineWidth', 0.8)
    hold on
    plot(x_sim_history(1, :), x_sim_history(2, :), 'b.', 'LineWidth', 0.5)
    hold on
    plot(xc_sim_history(1, :), xc_sim_history(2, :), 'g.', 'LineWidth', 0.5)
    hold on

    % 迭代绘制每一步的状态tube范围: 椭圆表示状态偏差的不确定性
    % P : 椭圆的协方差矩阵，表示椭圆的形状和方向。
    % mu * wmax_norm^2/lambda0 : 椭圆的半长轴长度，通过对参数进行线性组合计算得到。这个值决定了椭圆的大小。
    % [xn(1,i), xn(2,i)] : 椭圆的中心位置，即状态偏差的当前值。
    % 'y' : 椭圆的颜色，这里是黄色。
    %draw_ellip2(P1, mu * wmax_norm^2/lambda0, [x_sim(1), x_sim(2)] ,'y')
    draw_ellip2(P_nom, alpha_nom, [x_sim(1), x_sim(2)] ,'y')
    hold on

    %设置坐标轴名称的字体，可以覆盖上述设置
    xlabel('x (m)','fontsize',16);ylabel('y (m)','fontsize',16);
    %设置图例；'Orientation'设置图例为竖着还是横着，默认为竖着，'horizontal'为横
    legend({'ref','nomi-sim','tube-sim'},'Orientation','horizontal');
    %设置图例位置'NorthOutside'表示在上方外部；右上角为'NorthEast'，依此类推
    set(legend,'Location','NorthEast');
%     %设置x轴范围
%     xlim([-2,2]);
%     %设置x刻度如何显示
%     xticks(-2:1:2);
%     %设置y轴范围
%     ylim([-2,2]);
%     %设置y刻度如何显示
%     yticks(-2:1:2);
    axis equal;  % 保持坐标轴比例相等
    grid on;
end

%% plot
% 图2：x,y,theta状态随时间变化情况
figure;
subplot(3, 1, 1);
%'FontSize'设置所有的字的大小（刻度、坐标轴、图例等）
set(gca,'FontName','Times New Roman','FontSize',14);
plot(times(1:length(x_sim_history(1, :))), ref_x(1:length(x_sim_history(1, :))), 'r-', 'LineWidth', 0.8);
hold on;
plot(times(1:length(x_sim_history(1, :))), x_sim_history(1, :), 'b-', 'LineWidth', 0.8);
hold on;
plot(times(1:length(x_sim_history(1, :))), xc_sim_history(1, :), 'g--', 'LineWidth', 0.8);
ylabel('x (m)','fontsize',16);
%设置图例；'Orientation'设置图例为竖着还是横着，默认为竖着，'horizontal'为横
legend({'ref-x','nomi-x','tube-x'},'Orientation','horizontal');
%设置图例位置'NorthOutside'表示在上方外部；右上角为'NorthEast'，依此类推
set(legend,'Location','NorthOutside');
grid on;

subplot(3, 1, 2);
%'FontSize'设置所有的字的大小（刻度、坐标轴、图例等）
set(gca,'FontName','Times New Roman','FontSize',14);
plot(times(1:length(x_sim_history(2, :))), ref_y(1:length(x_sim_history(2, :))), 'r-', 'LineWidth', 0.8);
hold on;
plot(times(1:length(x_sim_history(2, :))), x_sim_history(2, :), 'b-', 'LineWidth', 0.8);
hold on;
plot(times(1:length(x_sim_history(2, :))), xc_sim_history(2, :), 'g--', 'LineWidth', 0.8);
ylabel('y (m)','fontsize',16);
%设置图例；'Orientation'设置图例为竖着还是横着，默认为竖着，'horizontal'为横
legend({'ref-y','nomi-y','tube-y'},'Orientation','horizontal');
%设置图例位置'NorthOutside'表示在上方外部；右上角为'NorthEast'，依此类推
set(legend,'Location','NorthOutside');
grid on;

subplot(3, 1, 3);
%'FontSize'设置所有的字的大小（刻度、坐标轴、图例等）
set(gca,'FontName','Times New Roman','FontSize',14);
plot(times(1:length(x_sim_history(3, :))), ref_theta(1:length(x_sim_history(3, :))), 'r-', 'LineWidth', 0.8);
hold on;
plot(times(1:length(x_sim_history(3, :))), x_sim_history(3, :), 'b-', 'LineWidth', 0.8);
hold on;
plot(times(1:length(x_sim_history(3, :))), xc_sim_history(3, :), 'g--', 'LineWidth', 0.8);
xlabel('时间 (s)','fontsize',16);ylabel('\theta (rad)','fontsize',16);
%设置图例；'Orientation'设置图例为竖着还是横着，默认为竖着，'horizontal'为横
legend({'ref-\theta','nomi-\theta','tube-\theta'},'Orientation','horizontal');
%设置图例位置'NorthOutside'表示在上方外部；右上角为'NorthEast'，依此类推
set(legend,'Location','NorthOutside');
grid on;

% 图3：v,omega输入随时间变化情况
figure;
subplot(2, 1, 1);
%'FontSize'设置所有的字的大小（刻度、坐标轴、图例等）
set(gca,'FontName','Times New Roman','FontSize',14);
plot(times(1:length(u_sim_history(1, :))), ref_v(1:length(u_sim_history(1, :))), 'r-', 'LineWidth', 0.8);
hold on;
plot(times(1:length(u_sim_history(1, :))), u_sim_history(1, :), 'b-', 'LineWidth', 0.8);
hold on;
plot(times(1:length(u_sim_history(1, :))), uc_sim_history(1, :), 'g--', 'LineWidth', 0.8);
ylabel('线速度 (m/s)','fontsize',16);
%设置图例；'Orientation'设置图例为竖着还是横着，默认为竖着，'horizontal'为横
legend({'ref-v','nomi-v','tube-v'},'Orientation','horizontal');
%设置图例位置'NorthOutside'表示在上方外部；右上角为'NorthEast'，依此类推
set(legend,'Location','NorthOutside');
grid on;

subplot(2, 1, 2);
%'FontSize'设置所有的字的大小（刻度、坐标轴、图例等）
set(gca,'FontName','Times New Roman','FontSize',14);
plot(times(1:length(ref_w)), ref_w(1,:), 'r-', 'LineWidth', 0.8);
hold on;
plot(times(1:length(ref_w)), u_sim_history(2, 1:length(ref_w)), 'b-', 'LineWidth', 0.8);
hold on;
plot(times(1:length(ref_w)), uc_sim_history(2, 1:length(ref_w)), 'g--', 'LineWidth', 0.8);
xlabel('时间 (s)','fontsize',16);ylabel('角速度 (rad/s)','fontsize',16);
%设置图例；'Orientation'设置图例为竖着还是横着，默认为竖着，'horizontal'为横
legend({'ref-\omega','nomi-\omega','tube-\omega'},'Orientation','horizontal');
%设置图例位置'NorthOutside'表示在上方外部；右上角为'NorthEast'，依此类推
set(legend,'Location','NorthOutside');
grid on;

% 图4：实际轨迹与参考轨迹之差 tube-ref
len = length(xc_sim_history);
tube_delta_x = ref_x(1:len) - xc_sim_history(1, :);
tube_delta_y = ref_y(1:len) - xc_sim_history(2, :);
tube_delta_theta = ref_theta(1:len) - xc_sim_history(3, :);
figure;
subplot(3, 1, 1);
%'FontSize'设置所有的字的大小（刻度、坐标轴、图例等）
set(gca,'FontName','Times New Roman','FontSize',14);
plot(times(1:len), tube_delta_x(1:len), 'r--', 'LineWidth', 0.8);
ylabel('\deltax (m)','fontsize',16);
%设置图例；'Orientation'设置图例为竖着还是横着，默认为竖着，'horizontal'为横
legend({'\deltax'},'Orientation','horizontal');
%设置图例位置'NorthOutside'表示在上方外部；右上角为'NorthEast'，依此类推
set(legend,'Location','NorthOutside');
grid on;

subplot(3, 1, 2);
%'FontSize'设置所有的字的大小（刻度、坐标轴、图例等）
set(gca,'FontName','Times New Roman','FontSize',14);
plot(times(1:len), tube_delta_y(1:len), 'g--', 'LineWidth', 0.8);
ylabel('\deltay (m)','fontsize',16);
%设置图例；'Orientation'设置图例为竖着还是横着，默认为竖着，'horizontal'为横
legend({'\deltay'},'Orientation','horizontal');
%设置图例位置'NorthOutside'表示在上方外部；右上角为'NorthEast'，依此类推
set(legend,'Location','NorthOutside');
grid on;

subplot(3, 1, 3);
%'FontSize'设置所有的字的大小（刻度、坐标轴、图例等）
set(gca,'FontName','Times New Roman','FontSize',14);
plot(times(1:len), tube_delta_theta(1:len), 'b--', 'LineWidth', 0.8);
xlabel('时间 (s)','fontsize',16);ylabel('\delta\theta (rad)','fontsize',16);
%设置图例；'Orientation'设置图例为竖着还是横着，默认为竖着，'horizontal'为横
legend({'\delta\theta'},'Orientation','horizontal');
%设置图例位置'NorthOutside'表示在上方外部；右上角为'NorthEast'，依此类推
set(legend,'Location','NorthOutside');
grid on;

% 图5：噪声
len=len-1;
figure;
subplot(3, 1, 1);
%'FontSize'设置所有的字的大小（刻度、坐标轴、图例等）
set(gca,'FontName','Times New Roman','FontSize',14);
plot(times(1:len), w(1,1:len), 'r--', 'LineWidth', 0.8);
ylabel('wx (m)','fontsize',16);
%设置图例；'Orientation'设置图例为竖着还是横着，默认为竖着，'horizontal'为横
legend({'wx'},'Orientation','horizontal');
%设置图例位置'NorthOutside'表示在上方外部；右上角为'NorthEast'，依此类推
set(legend,'Location','NorthOutside');
grid on;

subplot(3, 1, 2);
%'FontSize'设置所有的字的大小（刻度、坐标轴、图例等）
set(gca,'FontName','Times New Roman','FontSize',14);
plot(times(1:len), w(2,1:len), 'g--', 'LineWidth', 0.8);
ylabel('wy (m)','fontsize',16);
%设置图例；'Orientation'设置图例为竖着还是横着，默认为竖着，'horizontal'为横
legend({'wy'},'Orientation','horizontal');
%设置图例位置'NorthOutside'表示在上方外部；右上角为'NorthEast'，依此类推
set(legend,'Location','NorthOutside');
grid on;

subplot(3, 1, 3);
%'FontSize'设置所有的字的大小（刻度、坐标轴、图例等）
set(gca,'FontName','Times New Roman','FontSize',14);
plot(times(1:len), w(3,1:len), 'b--', 'LineWidth', 0.8);
xlabel('时间 (s)','fontsize',16);ylabel('w\theta (rad)','fontsize',16);
%设置图例；'Orientation'设置图例为竖着还是横着，默认为竖着，'horizontal'为横
legend({'w\theta'},'Orientation','horizontal');
%设置图例位置'NorthOutside'表示在上方外部；右上角为'NorthEast'，依此类推
set(legend,'Location','NorthOutside');
grid on;